#pragma once

#include <assert.h>
#include <memory.h>

#define FARF_HIGH 1
#include <HAP_farf.h>

#include <qhcomplex.h>
#include <qhmath.h>
#include <qhdsp.h>
#include <qhblas.h>


template<typename T>
struct CvPoint2
{
    CvPoint2() = default;
    CvPoint2(T x_, T y_):x(x_),y(y_){}
    T x;
    T y;
};

typedef CvPoint2<float> CvPoint2f;


template<typename T>
struct CvVec2
{
    CvVec2() = default;
    CvVec2(T x_, T y_):x(x_),y(y_){}
    T x;
    T y;
};

template<typename T>
struct CvRect_
{
    CvRect_() = default;
    CvRect_(T x_, T y_, T w, T h):x(x_),y(y_),width(w),height(h){}
    T x;
    T y;
    T width;
    T height;
};

typedef CvRect_<int32_t> CvRect;
typedef CvRect_<float> CvRectf;

template<typename T>
bool operator != (const CvRect_<T> &lh, const CvRect_<T> &rh) {
    return !(lh.x == rh.x &&
            lh.y == rh.y &&
            lh.width == rh.width &&
            lh.height == rh.height);
}

template <typename T, int alginN = 1024>
class CvMat
{
public:
    explicit CvMat():data(nullptr),rows(-1),cols(-1),stride(0),mem_ptr(nullptr),ref_cnt(nullptr) {
    }
    explicit CvMat(int rows_, int cols_, int stride=-1): CvMat() {
        resize(rows_, cols_, stride);
    }
    explicit CvMat(int size): CvMat(1, size) {}

    CvMat(const CvMat & h) {
        if (this != &h) {
            this->data = h.data;
            this->cols = h.cols;
            this->rows = h.rows;
            this->stride = h.stride;
            this->mem_ptr = h.mem_ptr;
            this->ref_cnt = h.ref_cnt;
            ref_cnt[0]++;
        }
    }
    CvMat& operator = (const CvMat &h) {
        if (this != &h) {
            if (ref_cnt && ref_cnt[0]) {
                ref_cnt[0]--;
                if ((ref_cnt[0] == 0) && mem_ptr) {
    //                FARF(RUNTIME_HIGH, "---d--operator==  %p, %d\n", data, ref_cnt[0]);
                    free(mem_ptr);
                }
            }
            this->data = h.data;
            this->mem_ptr = h.mem_ptr;
            this->ref_cnt = h.ref_cnt;
            this->rows = h.rows;
            this->cols = h.cols;
            this->stride = h.stride;
            ref_cnt[0]++;
//            FARF(RUNTIME_HIGH, "-----operator== %d\n", ref_cnt[0]);
        }
        return *this;
    }

    ~CvMat() {
        if(ref_cnt[0]) {
        	ref_cnt[0]--;
        	//        FARF(RUNTIME_HIGH, "----- ~CvMat %p %d\n", data, ref_cnt[0]);
        	if ((ref_cnt[0] == 0) && mem_ptr) {
        	//            FARF(RUNTIME_HIGH, "----- ~CvMat delete %p\n", data);
        	      free(mem_ptr);
        	}
        }
    }
 
    void zeros() {
        memset(data, 0, rows*stride);
    }   

    T& operator[](int i) const noexcept {
        // assert(i < rows*cols);
        return data[i];
    }

    T& operator()(int row, int col) const noexcept {
    	return *(T*)(((char*)data)+rows*stride+col*sizeof(T));
//        return this->operator[](row*cols+col);
    }

    T& operator()(int i) const noexcept {
        return this->operator[](i);
    }

    template<typename O = T>
    O* ptr() const noexcept {
        return (O*)data;
    }
    int colStride() const noexcept {
        return stride;
    }
    void resize(int rows_, int cols_, int stride_ = -1) {
        if (stride_ == -1) {
        	stride = cols_*sizeof(T);
        }
        if (data && rows_*cols_ <= rows*cols) {
            reshape(rows_, cols_, stride_);
        } else {
            void * prev = mem_ptr;
            int sz = ((rows_*stride+0x03)&~0x03);
            mem_ptr = memalign(alginN, sz+sizeof(int));
            assert(mem_ptr != NULL);
//            FARF(RUNTIME_HIGH, "-----resize %dx%d , %p, ref_cnt=%d\n", rows_, cols_, data, ref_cnt[0]);
            if (prev) {
                free(prev);
            }
            data = (T*)mem_ptr;
            rows = rows_;
            cols = cols_;
            ref_cnt = (int*)(((char*)mem_ptr)+sz);
            ref_cnt[0]++;
        }
    }

    void reshape(int rows_, int cols_, int stride_ = -1) {
        assert(rows_*cols_ <= cols*rows);
        rows = rows_;
        cols = cols_;
        if (stride_ == -1) {
        	stride = cols*sizeof(T);
        }
    }

    T *data;
    int rows;
    int cols;
    int stride;
private:
    void *mem_ptr;
    int *ref_cnt;
};

template<typename T>
T deg2rad(T deg) {
    return deg * static_cast<T>(M_PI/180.0);
}

template<typename T>
T rad2deg(T deg) {
    return deg * static_cast<T>(180.0/M_PI);
}

typedef CvMat<float> CvMatF32;
typedef CvMat<uint8_t> CvMatU8;
typedef CvMat<uint16_t> CvMatU16;
typedef CvMat<uint32_t> CvMatU32;

typedef CvMat<float complex> CvMatF32Complex;

// void splitCvMatF32Complex(CvMatF32Complex &a, CvMatF32 &real, CvMatF32 &imag)
// {
//     real.resize(a.rows, a.cols);
//     imag.resize(a.rows, a.cols);
//     for (int r = 0; r < a.rows; r++) {
//         for (int c = 0; c < a.cols; c++) {
//             real(r, c) = qhcomplex_creal_f(a(r, c));
//             imag(r, c) = qhcomplex_cimag_f(a(r, c));
//         }
//     }
// }


